#include "includes.h"

const u8 LinkCmd9141[7] = 
{
    6,
    0x68, 0x6a, 0xf1, 0x01, 0x00, 0xc4
};

u8 DSCmd9141[7] =
{
    6,
    0x68, 0x6a, 0xf1, 0x01, 0x04, 0xc8
};

const u8 DTCCmd9141[7] =
{
    5,
    0x68, 0x6a, 0xf1, 0x03, 0xc6
};

const u8 CDTCCmd9141[6] =
{
    5,
    0x68, 0x6a, 0xf1, 0x04, 0xc7
};

int iso9141_2addr_link(void)
{
    int ret = -1;
    ErrorStatus err;
    u8 i;

    printk("Link...\r\n");
#if 0
    for(i = 0; i < 3; i++) {
        while(wakeup_addr(0x33) == SUCCESS) {
            printk("iso9141 wake up\r\n");
            break;
        }
        OSTimeDly(OS_TICKS_PER_SEC / 2);
    }

    if(i >= 3) {
        printk("iso9141 failed to wake up\r\n");
        return -1;
    }
#endif
//    if(wakeup_addr(0x33) == SUCCESS) {
        for(i = 0; i < 2; i++) {
            send_iso9141_frame((u8 *)LinkCmd9141, &err);
            OSTimeDly(OS_TICKS_PER_SEC * 8);
            if(err == SUCCESS) {
                ret = 0;
                printk("OK\r\n");
                break;
            } else if(err == ERROR && i >= 1) {
                ret = -1;
                printk("Link failed\r\n");
            }
        }
//    }

    return ret;
}

s16 iso9141_2addr_readEnginSpeed(void)
{
    ErrorStatus err;
    s16 val;
    u8 *ram;

    DSCmd9141[5] = 0x0c;
    ram = send_iso9141_frame((u8 *)DSCmd9141, &err);
    if(err == SUCCESS) {
        val = ((ram[5] << 8) | ram[6]) / 4;
    } else {
        val = -1;
    }

    return val;
}

u8 iso9141_get_error_code(u16 *codes)
{
    u8 *ram, i;
    u16 dtc;
    u8 num = 0;
    ErrorStatus err;

    ram = send_iso9141_frame((u8 *)&DTCCmd9141, &err);
    if(err == SUCCESS) {
        for(i = 0; i < 10; i++) {
            dtc = (u16)(*(ram + 4 + 2 * i) << 8 |
                    *(ram + 5 + 2 * i));
            if(dtc != 0) {
                codes[num ++] = dtc;
            }
        }
    } else {
        num = 0xff;
        printk("%s: failed to do dtc\r\n", __func__);
    }

    return num;
}

int iso9141_clear_error_code(void)
{
    int ret;
    ErrorStatus err;

    send_iso9141_frame((u8 *)&CDTCCmd9141, &err);
    if(err == SUCCESS) {
        ret = 0;
    } else {
        printk("%s: failed to do dtc\r\n", __func__);
        ret = -1;
    }

    return ret;
}

u8 iso9141_2addr_get_payload(void)
{
    ErrorStatus err;
    u8 val;
    u8 *ram;

    DSCmd9141[5] = 0x04;
    ram = send_iso9141_frame((u8 *)DSCmd9141, &err);
    if(err == SUCCESS) {
        val = ram[5] * 100 / 255;
    } else {
        val = 0xff;
    }

    return val;
}

u8 iso9141_2addr_get_ect(void)
{
    ErrorStatus err;
    u8 val;
    u8 *ram;

    DSCmd9141[5] = 0x05;
    ram = send_iso9141_frame((u8 *)DSCmd9141, &err);
    if(err == SUCCESS) {
        val = ram[5] - 40;
    } else {
        val = 0xff;
    }

    return val;
}

u16 iso9141_2addr_get_speed(void)
{
    ErrorStatus err;
    u16 val;
    u8 *ram;

    DSCmd9141[5] = 0x0d;
    ram = send_iso9141_frame((u8 *)DSCmd9141, &err);
    if(err == SUCCESS) {
        val = ram[5];
    } else {
        val = 0xffff;
    }

    return val;
}

u16 iso9141_2addr_get_mil_distance(void)
{
    ErrorStatus err;
    u16 val;
    u8 *ram;

    DSCmd9141[5] = 0x21;
    ram = send_iso9141_frame((u8 *)DSCmd9141, &err);
    if(err == SUCCESS) {
        val = (ram[5] << 8) | ram[6];
    } else {
        val = 0xffff;
    }

    return val;
}

float iso9141_2addr_get_relative_frp(void)
{
    float val = -1;

    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_frp(void)
{
    ErrorStatus err;
    float val;
    u8 *ram;

    DSCmd9141[5] = 0x0a;
    ram = send_iso9141_frame((u8 *)DSCmd9141, &err);
    if(err == SUCCESS) {
        val = ram[5] * 3;
    } else {
        val = -1;
    }

    return val;
}

float iso9141_2addr_get_ratio_1_1(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_o2vol_1_1(void)
{
    ErrorStatus err;
    float val;
    u8 *ram;

    DSCmd9141[5] = 0x14;
    ram = send_iso9141_frame((u8 *)DSCmd9141, &err);
    if(err == SUCCESS) {
        val = ram[5] * 0.005;
    } else {
        val = -1;
    }

    return val;
}

float iso9141_2addr_get_ratio_1_2(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_o2vol_1_2(void)
{
    ErrorStatus err;
    float val;
    u8 *ram;

    DSCmd9141[5] = 0x15;
    ram = send_iso9141_frame((u8 *)DSCmd9141, &err);
    if(err == SUCCESS) {
        val = ram[5] * 0.005;
    } else {
        val = -1;
    }

    return val;
}

float iso9141_2addr_get_ratio_1_3(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_o2vol_1_3(void)
{
    ErrorStatus err;
    float val;
    u8 *ram;

    DSCmd9141[5] = 0x16;
    ram = send_iso9141_frame((u8 *)DSCmd9141, &err);
    if(err == SUCCESS) {
        val = ram[5] * 0.005;
    } else {
        val = -1;
    }

    return val;
}

float iso9141_2addr_get_ratio_1_4(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_o2vol_1_4(void)
{
    ErrorStatus err;
    float val;
    u8 *ram;

    DSCmd9141[5] = 0x17;
    ram = send_iso9141_frame((u8 *)DSCmd9141, &err);
    if(err == SUCCESS) {
        val = ram[5] * 0.005;
    } else {
        val = -1;
    }

    return val;
}

float iso9141_2addr_get_ratio_2_1(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_o2vol_2_1(void)
{
    ErrorStatus err;
    float val;
    u8 *ram;

    DSCmd9141[5] = 0x18;
    ram = send_iso9141_frame((u8 *)DSCmd9141, &err);
    if(err == SUCCESS) {
        val = ram[5] * 0.005;
    } else {
        val = -1;
    }

    return val;
}

float iso9141_2addr_get_ratio_2_2(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_o2vol_2_2(void)
{
    ErrorStatus err;
    float val;
    u8 *ram;

    DSCmd9141[5] = 0x19;
    ram = send_iso9141_frame((u8 *)DSCmd9141, &err);
    if(err == SUCCESS) {
        val = ram[5] * 0.005;
    } else {
        val = -1;
    }

    return val;
}

float iso9141_2addr_get_ratio_2_3(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_o2vol_2_3(void)
{
    ErrorStatus err;
    float val;
    u8 *ram;

    DSCmd9141[5] = 0x1A;
    ram = send_iso9141_frame((u8 *)DSCmd9141, &err);
    if(err == SUCCESS) {
        val = ram[5] * 0.005;
    } else {
        val = -1;
    }

    return val;
}

float iso9141_2addr_get_ratio_2_4(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_o2vol_2_4(void)
{
    ErrorStatus err;
    float val;
    u8 *ram;

    DSCmd9141[5] = 0x1B;
    ram = send_iso9141_frame((u8 *)DSCmd9141, &err);
    if(err == SUCCESS) {
        val = ram[5] * 0.005;
    } else {
        val = -1;
    }

    return val;
}

float iso9141_2addr_get_egr(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_egr_error(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_ioep(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_fuel_level_input(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

u16 iso9141_2addr_get_after_start_times(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

u16 iso9141_2addr_get_after_distance(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

u16 iso9141_2addr_get_emission_sys_press(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

u16 iso9141_2addr_get_press(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_o2cur_1_1(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_ratio2_1_1(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_o2cur_1_2(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_ratio2_1_2(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_o2cur_1_3(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_ratio2_1_3(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_o2cur_1_4(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_ratio2_1_4(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_o2cur_2_1(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_ratio2_2_1(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_o2cur_2_2(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_ratio2_2_2(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_o2cur_2_3(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_ratio2_2_3(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_o2cur_2_4(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_ratio2_2_4(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_cat_temp_1_1(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_cat_temp_1_2(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_cat_temp_2_1(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_cat_temp_2_2(void)
{
    float val = -1;
    
    printk("Unsupported!\r\n");

    return val;
}

float iso9141_2addr_get_short_fuel_adj_1(void)
{
    ErrorStatus err;
    float val;
    u8 *ram;

    DSCmd9141[5] = 0x06;
    ram = send_iso9141_frame((u8 *)DSCmd9141, &err);
    if(err == SUCCESS) {
        val = ram[5] * 100 / 128 - 100;
    } else {
        val = -1;
    }

    return val;
}

float iso9141_2addr_get_long_fuel_adj_1(void)
{
    ErrorStatus err;
    float val;
    u8 *ram;

    DSCmd9141[5] = 0x07;
    ram = send_iso9141_frame((u8 *)DSCmd9141, &err);
    if(err == SUCCESS) {
        val = ram[5] * 100 / 128 - 100;
    } else {
        val = -1;
    }

    return val;
}

float iso9141_2addr_get_short_fuel_adj_2(void)
{
    ErrorStatus err;
    float val;
    u8 *ram;

    DSCmd9141[5] = 0x08;
    ram = send_iso9141_frame((u8 *)DSCmd9141, &err);
    if(err == SUCCESS) {
        val = ram[5] * 100 / 128 - 100;
    } else {
        val = -1;
    }

    return val;
}

float iso9141_2addr_get_long_fuel_adj_2(void)
{
    ErrorStatus err;
    float val;
    u8 *ram;

    DSCmd9141[5] = 0x09;
    ram = send_iso9141_frame((u8 *)DSCmd9141, &err);
    if(err == SUCCESS) {
        val = ram[5] * 100 / 128 - 100;
    } else {
        val = -1;
    }

    return val;
}

u8 iso9141_2addr_get_gas_temp(void)
{
    ErrorStatus err;
    u8 val;
    u8 *ram;

    DSCmd9141[5] = 0x06;
    ram = send_iso9141_frame((u8 *)DSCmd9141, &err);
    if(err == SUCCESS) {
        val = ram[5] - 40;
    } else {
        val = 0xff;
    }

    return val;
}

float iso9141_2addr_get_throttle_pos(void)
{
    ErrorStatus err;
    float val;
    u8 *ram;

    DSCmd9141[5] = 0x11;
    ram = send_iso9141_frame((u8 *)DSCmd9141, &err);
    if(err == SUCCESS) {
        val = ram[5] * 100 / 255;
    } else {
        val = -1;
    }

    return val;
}

